#include "ePuck.h"
#include <stdio.h>
#include <pthread.h>

void
PrintResult(  const ePuck::EResult Result, 
              const std::string& rCustom )
{
  if( ePuck::EResultOk == Result )
  {
    printf( " [ OK ] %s successfull\n", 
      rCustom.c_str() );
  }
  else
  {
    printf( " [FAIL] %s, failed with code:%d\n", 
      rCustom.c_str(), Result );
  }
  fflush(stdout);
}

int main(int argc, char** argv)
{
  ePuck::EResult Result = ePuck::EResultOk;

  // Create ePuck device
  ePuck::Device ePuck;

  // Connect to device
  if( ePuck::EResultOk == Result )
  {
    Result = ePuck.Connect( "/dev/rfcomm0" );
    PrintResult( Result, "Connection" );
  }
  
  // Read proximity sensors
  if( 0 )
  {
    ePuck::SProximity Proximity;
    Result = ePuck.GetProximity( Proximity );
    PrintResult( Result, "Get Proximity" );
    printf
    ( 
      "%u %u %u %u %u %u %u %u\n"
    , Proximity.S0
    , Proximity.S1
    , Proximity.S2
    , Proximity.S3
    , Proximity.S4
    , Proximity.S5
    , Proximity.S6
    , Proximity.S7
    );
  }
  
  // Start moving
  if( ePuck::EResultOk == Result )
  {
    Result = ePuck.Move( 100, 100 );
    PrintResult( Result, "Move" );
    sleep( 3 );
  }
  
  // Read proximity sensors
  if( 0 )
  {
    ePuck::SProximity Proximity;
    Result = ePuck.GetProximity( Proximity );
    PrintResult( Result, "Get Proximity" );
    printf
    ( 
      "%u %u %u %u %u %u %u %u\n"
    , Proximity.S0
    , Proximity.S1
    , Proximity.S2
    , Proximity.S3
    , Proximity.S4
    , Proximity.S5
    , Proximity.S6
    , Proximity.S7
    );
  }
  
  // Turn on LED
  if( ePuck::EResultOk == Result )
  {
    Result = ePuck.SetLed( ePuck::ELedIdFront, 
      ePuck::ELedStateOn );
    PrintResult( Result, "Front LED on" );
    sleep( 3 );
  }

  if( ePuck::EResultOk == Result )
  {
    Result = ePuck.Sound( ePuck::ESoundEuh );
    PrintResult( Result, "Sound Euh" );
    sleep( 3 );
  }

  if( ePuck::EResultOk == Result )
  {
    Result = ePuck.Sound( ePuck::ESoundWheeeh );
    PrintResult( Result, "Sound Wheeeh" );
    sleep( 3 );
  }

  if( ePuck::EResultOk == Result )
  {
    Result = ePuck.Sound( ePuck::ESoundOff );
    PrintResult( Result, "Sound Off" );
    sleep( 3 );
  }
  
  // Turn off LED
  if( ePuck::EResultOk == Result )
  {
    Result = ePuck.SetLed( ePuck::ELedIdFront, 
      ePuck::ELedStateOff );
    PrintResult( Result, "Front LED off" );
    sleep( 3 );
  }

  // Stop moving
  if( ePuck::EResultOk == Result )
  {
    Result = ePuck.Stop( );
    PrintResult( Result, "Stop" );
  }

  // Read proximity sensors
  if( ePuck::EResultOk == Result )
  {
    ePuck::SProximity Proximity;
    Result = ePuck.GetProximity( Proximity );
    PrintResult( Result, "Get Proximity" );
    printf
    ( 
      "%u %u %u %u %u %u %u %u\n"
    , Proximity.S0
    , Proximity.S1
    , Proximity.S2
    , Proximity.S3
    , Proximity.S4
    , Proximity.S5
    , Proximity.S6
    , Proximity.S7
    );
  }
}
